#include <stdlib.h>
#include <memory.h>
#include <stdio.h>

#include "LogicPool/LogicInterfaceVME.h"
#include "vme_sis.h"
extern "C" {
#include "sis3100_vme_calls.h"
}

#ifdef DEBUG
#include <iostream>
using namespace std;
#endif


static const uint32_t vme_sys_ofs  = (1 << 18);
static const uint32_t vme_high_adr = vme_sys_ofs + 8;
static const uint32_t vme_reset    = vme_sys_ofs + 4;
static const uint32_t vme_id       = vme_sys_ofs + 0;


void LogicInterfaceVME::Open()
{
}


void LogicInterfaceVME::Close()
{
}


void LogicInterfaceVME::Reset()
{
  vme_A24D32_write(handle, address + vme_reset, 1);
  vme_A24D32_write(handle, address + vme_reset, 0);
}


uint32_t LogicInterfaceVME::GetID()
{
  uint32_t data;
  vme_A24D32_read(handle, address +  vme_id, &data);
  return data;
}


void LogicInterfaceVME::SetAddress(uint8_t type, uint8_t number, uint8_t addr)
{
  lbaddress = (type << 16) | (number << 8) | addr;
#ifdef DEBUG
  cout << hex << "SetAddress: VME: 0x" << address + vme_high_adr << " LP: 0x" << lbaddress << " write: 0x" << (lbaddress >> 16);
#endif
  if (lb_high_adr_cache != (lbaddress >> 16)) {
    vme_A24D32_write(handle, address + vme_high_adr, lb_high_adr_cache = (lbaddress >> 16));
//    usleep(10000);  //FIXME: somehow this sleep seems to be necessary sometimes
  }
#ifdef DEBUG
  else
    cout << " (cached)";
  cout << endl;
#endif
}


void LogicInterfaceVME::SetShortAddress(uint8_t number, uint8_t addr)
{
  lbaddress = (number << 8) | addr;
}


uint8_t LogicInterfaceVME::ReadByte()
{
  uint32_t data;
  vme_A24D32_read(handle, address + 4*(lbaddress & 0xffff), &data);
  return data & 0xff;
}


void LogicInterfaceVME::WriteByte(uint8_t data)
{
  vme_A24D32_write(handle, address + 4*(lbaddress & 0xffff), data);
#ifdef DEBUG
  cout << hex << "WriteByte: VME: 0x" << address + 4*(lbaddress & 0xffff) << " LP: 0x" << lbaddress << " data: 0x" << hex << (uint32_t)data << endl;
#endif
 }


uint16_t LogicInterfaceVME::ReadWord()
{
  uint32_t data;
  vme_A24D32_read(handle, address + 4*(lbaddress & 0xffff), &data);
  return data & 0xffff;
}


void LogicInterfaceVME::ReadWordFIFO(uint16_t * data, uint16_t n)
{
  uint32_t i, m;
  uint32_t * data32;
  
  data32 = (uint32_t *) malloc(n * sizeof(uint32_t)); 
  vme_A24DMA_D32FIFO_read(handle, address + 4*(lbaddress & 0xffff), data32, n, &m);
  //FIXME: add a check for m==n here!
  for (i = 0; i < m; i++)
    data[i] = data32[i] & 0xffff;
  for (; i < n; i++)
    data[i] = 0;
  free(data32);
}


void LogicInterfaceVME::WriteWord(uint16_t data)
{
  vme_A24D32_write(handle, address + 4*(lbaddress & 0xffff), data);
}


uint32_t LogicInterfaceVME::ReadLong()
{
  uint32_t data;
  vme_A24D32_read(handle, address + 4*(lbaddress & 0xffff), &data);
  return data;
}
  
      
void LogicInterfaceVME::WriteLong(uint32_t data)
{
  vme_A24D32_write(handle, address + 4*(lbaddress & 0xffff), data);
}

